
#include "ros/ros.h"
#include <opencv2/opencv.hpp>
#include <geometry_msgs/Twist.h>

#include <iostream>
#include <chrono>

#include "../include/detector.h"

using namespace std;
using namespace cv;

cv::Mat src_frame; //全局图像
dip::Ex4Detector detector;//创建检测器

void red_hsv_callback(int, void*)
{	//调节红色hsv回调函数
	cv::Mat thre_img,hsv_img;
	cv::cvtColor(src_frame,hsv_img,cv::COLOR_BGR2HSV);
	cv::inRange(hsv_img,cv::Scalar(detector.hsv_thre_list[0].h_min,detector.hsv_thre_list[0].s_min,detector.hsv_thre_list[0].v_min), 
                cv::Scalar(detector.hsv_thre_list[0].h_max,detector.hsv_thre_list[0].s_max,detector.hsv_thre_list[0].v_max),thre_img); //Threshold the image 
	cv::imshow("red debug",thre_img);
}

void blue_hsv_callback(int, void*)
{	//调节蓝色hsv回调函数
	cv::Mat thre_img,hsv_img;
	cv::cvtColor(src_frame,hsv_img,cv::COLOR_BGR2HSV);
	cv::inRange(hsv_img,cv::Scalar(detector.hsv_thre_list[5].h_min,detector.hsv_thre_list[5].s_min,detector.hsv_thre_list[5].v_min), 
                cv::Scalar(detector.hsv_thre_list[5].h_max,detector.hsv_thre_list[5].s_max,detector.hsv_thre_list[5].v_max),thre_img); //Threshold the image 
	cv::imshow("blue debug",thre_img);
}

void green_hsv_callback(int, void*)
{	//调节绿色hsv回调函数
	cv::Mat thre_img,hsv_img;
	cv::cvtColor(src_frame,hsv_img,cv::COLOR_BGR2HSV);
	cv::inRange(hsv_img,cv::Scalar(detector.hsv_thre_list[3].h_min,detector.hsv_thre_list[3].s_min,detector.hsv_thre_list[3].v_min), 
                cv::Scalar(detector.hsv_thre_list[3].h_max,detector.hsv_thre_list[3].s_max,detector.hsv_thre_list[3].v_max),thre_img); //Threshold the image 
	cv::imshow("green debug",thre_img);
}

void yellow_hsv_callback(int, void*)
{	//调节红色hsv回调函数
	cv::Mat thre_img,hsv_img;
	cv::cvtColor(src_frame,hsv_img,cv::COLOR_BGR2HSV);
	cv::inRange(hsv_img,cv::Scalar(detector.hsv_thre_list[2].h_min,detector.hsv_thre_list[2].s_min,detector.hsv_thre_list[2].v_min), 
                cv::Scalar(detector.hsv_thre_list[2].h_max,detector.hsv_thre_list[2].s_max,detector.hsv_thre_list[2].v_max),thre_img); //Threshold the image 
	cv::imshow("yellow debug",thre_img);
}

void get_action(geometry_msgs::Twist& cmd,dip::EX4_COLOR color)
{
    if (color == dip::EX4_COLOR::NONE)
    { //检测不到不动
        cmd.linear.x = 0;
		cmd.linear.y = 0;
		cmd.linear.z = 0;
		cmd.angular.x = 0;
		cmd.angular.y = 0;
		cmd.angular.z = 0;
    }
	else if (color == dip::EX4_COLOR::RED)
	{ //
		 cmd.linear.x = 0.2;
		cmd.linear.y = 0;
		cmd.linear.z = 0;
		cmd.angular.x = 0;
		cmd.angular.y = 0;
		cmd.angular.z = 0;
	}
	else if (color == dip::EX4_COLOR::GREEN)
	{ //
		 cmd.linear.x = -0.2;
		cmd.linear.y = 0;
		cmd.linear.z = 0;
		cmd.angular.x = 0;
		cmd.angular.y = 0;
		cmd.angular.z = 0;
	}
	else if (color == dip::EX4_COLOR::BLUE)
	{ //
		 cmd.linear.x = 0;
		cmd.linear.y = 0;
		cmd.linear.z = 0;
		cmd.angular.x = 0;
		cmd.angular.y = 0;
		cmd.angular.z = 0.1;
	}
	else if (color == dip::EX4_COLOR::YELLOW)
	{ //
		 cmd.linear.x = 0;
		cmd.linear.y = 0;
		cmd.linear.z = 0;
		cmd.angular.x = 0;
		cmd.angular.y = 0;
		cmd.angular.z = -0.1;
	}
	else
	{
		cmd.linear.x = 0;
		cmd.linear.y = 0;
		cmd.linear.z = 0;
		cmd.angular.x = 0;
		cmd.angular.y = 0;
		cmd.angular.z = 0;
	}

}

int main(int argc, char **argv)
{
	ROS_INFO("*****DIP_EX4 START*****");
	ros::init(argc, argv, "label_detect"); //初始化ROS节点
	ros::NodeHandle n;

	//Before the use of camera, you can test ur program with images first: imread()
	VideoCapture capture;
	// capture.open("/home/lin/桌面/rect.mp4"); //打开zed相机，如果要打开笔记本上的摄像头，需要改为0
	capture.open(0);
	waitKey(100);
	if (!capture.isOpened())
	{
		std::cerr<<"open camera error"<<std::endl;
		return 1;
	}

    //publisher
	ros::Publisher pub = n.advertise<geometry_msgs::Twist>("/smoother_cmd_vel", 5); //定义dashgo机器人的速度发布器

    //DIP
    cv::namedWindow("red hsv threshold");
	cv::createTrackbar("h_min", "red hsv threshold",&(detector.hsv_thre_list[0].h_min), 255, red_hsv_callback,0);
	cv::createTrackbar("h_max", "red hsv threshold",&(detector.hsv_thre_list[0].h_max), 255, red_hsv_callback,0);
	cv::createTrackbar("s_min", "red hsv threshold",&(detector.hsv_thre_list[0].s_min), 255, red_hsv_callback,0);
	cv::createTrackbar("s_max", "red hsv threshold",&(detector.hsv_thre_list[0].s_max), 255, red_hsv_callback,0);
	cv::createTrackbar("v_min", "red hsv threshold",&(detector.hsv_thre_list[0].v_min), 255, red_hsv_callback,0);
	cv::createTrackbar("v_max", "red hsv threshold",&(detector.hsv_thre_list[0].v_max), 255, red_hsv_callback,0);

	//yellow 
	cv::namedWindow("yellow hsv threshold");
	cv::createTrackbar("h_min", "yellow hsv threshold",&(detector.hsv_thre_list[2].h_min), 255, yellow_hsv_callback,0);
	cv::createTrackbar("h_max", "yellow hsv threshold",&(detector.hsv_thre_list[2].h_max), 255, yellow_hsv_callback,0);
	cv::createTrackbar("s_min", "yellow hsv threshold",&(detector.hsv_thre_list[2].s_min), 255, yellow_hsv_callback,0);
	cv::createTrackbar("s_max", "yellow hsv threshold",&(detector.hsv_thre_list[2].s_max), 255, yellow_hsv_callback,0);
	cv::createTrackbar("v_min", "yellow hsv threshold",&(detector.hsv_thre_list[2].v_min), 255, yellow_hsv_callback,0);
	cv::createTrackbar("v_max", "yellow hsv threshold",&(detector.hsv_thre_list[2].v_max), 255, yellow_hsv_callback,0);

	//green 
	cv::namedWindow("green hsv threshold");
	cv::createTrackbar("h_min", "green hsv threshold",&(detector.hsv_thre_list[3].h_min), 255, green_hsv_callback,0);
	cv::createTrackbar("h_max", "green hsv threshold",&(detector.hsv_thre_list[3].h_max), 255, green_hsv_callback,0);
	cv::createTrackbar("s_min", "green hsv threshold",&(detector.hsv_thre_list[3].s_min), 255, green_hsv_callback,0);
	cv::createTrackbar("s_max", "green hsv threshold",&(detector.hsv_thre_list[3].s_max), 255, green_hsv_callback,0);
	cv::createTrackbar("v_min", "green hsv threshold",&(detector.hsv_thre_list[3].v_min), 255, green_hsv_callback,0);
	cv::createTrackbar("v_max", "green hsv threshold",&(detector.hsv_thre_list[3].v_max), 255, green_hsv_callback,0);
	

	//blue 
	cv::namedWindow("blue hsv threshold");
	cv::createTrackbar("h_min", "blue hsv threshold",&(detector.hsv_thre_list[5].h_min), 255, blue_hsv_callback,0);
	cv::createTrackbar("h_max", "blue hsv threshold",&(detector.hsv_thre_list[5].h_max), 255, blue_hsv_callback,0);
	cv::createTrackbar("s_min", "blue hsv threshold",&(detector.hsv_thre_list[5].s_min), 255, blue_hsv_callback,0);
	cv::createTrackbar("s_max", "blue hsv threshold",&(detector.hsv_thre_list[5].s_max), 255, blue_hsv_callback,0);
	cv::createTrackbar("v_min", "blue hsv threshold",&(detector.hsv_thre_list[5].v_min), 255, blue_hsv_callback,0);
	cv::createTrackbar("v_max", "blue hsv threshold",&(detector.hsv_thre_list[5].v_max), 255, blue_hsv_callback,0);
    
	std::vector<int> pixel_counts;//像素统计
	std::vector<cv::Mat> hsv_thre_imgs;
	bool enable_debug = false;
	while (ros::ok())
	{
		capture.read(src_frame);
		if (src_frame.empty())
		{
			break;
		}
		cv::resize(src_frame,src_frame,Size(640,480));

        dip::EX4_COLOR color =  detector.detect_label(src_frame,hsv_thre_imgs,pixel_counts,enable_debug);
		imshow("src", src_frame);
        if (hsv_thre_imgs.size() >1 && enable_debug)
		{
			// std::cout<<"hsv seg num"<<hsv_thre_imgs.size()<<std::endl;
			imshow("hsv seg red",hsv_thre_imgs[0]);
			imshow("hsv seg yellow",hsv_thre_imgs[1]);
			imshow("hsv seg green",hsv_thre_imgs[2]);
			imshow("hsv seg blue",hsv_thre_imgs[3]);
			imshow("hsv",hsv_thre_imgs[4]);
			
		}
		std::cout<<"label color"<<(int)color<<std::endl;

		//以下代码可设置机器人的速度值，从而控制机器人运动
		geometry_msgs::Twist chasis_cmd;
		get_action(chasis_cmd,color);
		pub.publish(chasis_cmd);

		ros::spinOnce();
		waitKey(5);
	}
	return 0;
}